#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright:  Jihua Lab 2021.
@File    :   sea.py    
@Author :   HaoWANG, Foshan，China
@Contact :   wanghao@jihualab.com
@License :   JHL ( not BSD)

@Description:  


@Create Time   :   2022/6/8 0008  9:19
@Modify Time      @Author    @Version    @Desciption
------------      -------    --------    -----------
2022/6/8 0008 9:19   wanghao      1.1.0         None
20220608 Update the SEA angle and force detection method and ADS12263 Modual usage for Python3/ Python2
        Testing in Rpi4B - PASS
        Jetson Xavier NX - ING

20220914 TODO: Three Threads: 1 - Dual arm motion control 
                2 - records the left pitch joint angle state 
                3- records the right pitch joint angle state 

'''

# import lib
import time
import ADS1263
import queue


class MovingAverage:
    def __init__(self, size: int):
        self.que = queue.Queue(size)

    def next(self, val: float):
        if (self.que.full()):
            self.que.get()  # get the queue item value
        self.que.put(val)
        return sum(self.que.queue) / self.que.qsize()  # calculate the average voltage value


def main():
    # test main function
    REF = 5.08  # Modify according to actual voltage
    # external AVDD and AVSS(Default), or internal 2.5V

    servo_list = [0, 1, 4, 5]  # 0,1 left arm ;4,5 right arm shoulder joint yaw and pitch, respectively

    SEA_Balanced_Angle = []  # static information of sea joint balanced angle

    # def average size
    kernel_size = 3
    frequency = 20  # Hz
    moving_average_a = MovingAverage(kernel_size)
    moving_average_b = MovingAverage(kernel_size)

    current_yaw_right_buffer = []
    current_pitch_right_buffer = []

    try:
        ADC = ADS1263.ADS1263()
        if (ADC.ADS1263_init_ADC1('ADS1263_7200SPS') == -1):
            exit()
        ADC.ADS1263_SetMode(0)

        # ADC.ADS1263_DAC_Test(1, 1)      # Open IN6
        # ADC.ADS1263_DAC_Test(0, 1)      # Open IN7

        if (ADC.ADS1263_init_ADC2('ADS1263_ADC2_400SPS') == -1):
            exit()
        while (1):
            ADC_Value = ADC.ADS1263_GetAll_ADC2()  # get ADC2 value
            for i in range(0 , 1 ):
                if i == 0: # IN0 ADC_IN0 Value
                    if (ADC_Value[i] >> 23 == 1):
                        val = REF * 2 - ADC_Value[i] * REF / 0x800000
                        avg = moving_average_a.next(val)
                        print("ADC2 IN%d = -%lf" % (i, (REF * 2 - ADC_Value[i] * REF / 0x800000)))
                        print("     AVG a : ADC2 IN%d = -%lf" % (i, avg))
                    else:
                        val = ADC_Value[i] * REF / 0x7fffff
                        avg = moving_average_a.next(val)
                        print("ADC2 IN%d = %lf" % (i, (ADC_Value[i] * REF / 0x7fffff)))  # 24bit
                        print("     AVG a : ADC2 IN%d = %lf" % (i, avg))
                    angle = (360.0 * avg) / REF  # degree of sea

                    current_yaw_right_buffer.append(angle)
                    print("SEA  ID : {}   - ANGLE : ".format(i)+ format(angle, '.3f'))
                    
                else:
                    if (ADC_Value[i] >> 23 == 1):
                        val_b = REF * 2 - ADC_Value[i] * REF / 0x800000
                        avg_b = moving_average_b.next(val_b)
                        print("ADC2 IN%d = -%lf" % (i, (REF * 2 - ADC_Value[i] * REF / 0x800000)))
                        print("     AVG a : ADC2 IN%d = -%lf" % (i, avg_b))
                    else:
                        val_b = ADC_Value[i] * REF / 0x7fffff
                        avg_b = moving_average_b.next(val_b)
                        print("ADC2 IN%d = %lf" % (i, (ADC_Value[i] * REF / 0x7fffff)))  # 24bit
                        print("     AVG a : ADC2 IN%d = %lf" % (i, avg_b))
                    angle_b = (360.0 * avg_b) / REF  # degree of sea

                    current_pitch_right_buffer.append(angle_b)
                    print("SEA  ID : {}   - ANGLE : ".format(i)+ format(angle, '.3f'))
            time.sleep(1 / frequency)

        ADC.ADS1263_Exit()

    except IOError as e:
        print(e)

    except KeyboardInterrupt:
        print("ctrl + c:")
        print("Program end")
        print(current_yaw_right_buffer,current_pitch_right_buffer)
        ADC.ADS1263_Exit()
        exit()


if __name__ == "__main__":
    main()
